package Modelos;
import Escenario.*;


public class ModeloAceleracionSeguimientoDeVehiculo implements ModeloComportamiento{
	
	private static double alpha_bigger =2.15d;
	private static double l_bigger =-0.88d;
	private static double m_bigger=-1.67d;
	
	private static double alpha_lower =1.55d;
	private static double l_lower =1.66d;
	private static double m_lower=1.08d;
	
	public EscenarioImpl aplicaModelo(EscenarioImpl esc, ParCV actual, Integer numCarril, Integer posParCV){
		
		double velseguidor, velprecedente,velprecedentenueva, 
				distinfl, distancia,acelnueva, velnueva,incrementotiempo, velocidaddeseada;
		
		double posseguidor, posprecedente, posnueva;
		
		ParCV precedente = actual.getSiguiente();
		velseguidor = actual.getEstadoActual().getVelocidad();
		velprecedente = precedente.getEstadoActual().getVelocidad();		
		velprecedentenueva = precedente.getEstadoNuevo().getVelocidad();
		velocidaddeseada = actual.getConductor().getVelocidadDeFlujoLibre();
		
		posseguidor = actual.getEstadoActual().getPosicion();
		posprecedente = precedente.getEstadoActual().getPosicion();
		incrementotiempo = actual.getIncrementoTiempo();
		
		distinfl = actual.getDistanciaInfluenciaDinamica();
		distancia = actual.getDistanciaActual();

		double den=0;
		double num=0;
		double prueba=0;
		System.out.println("CASO: seguidor ("+actual+") y precedente: ("+precedente+")");
		
		System.out.println("Distancia: "+distancia+" DistanciaInfluencia: "+distinfl);
		if(distancia<distinfl){ //necesita disminuir la aceleracion
			
			//prueba de fiabilidad de la ecuacion
			num = (alpha_bigger*Math.pow(velprecedentenueva, m_bigger)*(velseguidor-velprecedente));
			den = (Math.pow(posprecedente-posseguidor, l_bigger));
			prueba = num / den;
			
			acelnueva = (alpha_bigger*Math.pow(velprecedentenueva, m_bigger)*(velseguidor-velprecedente)) / (Math.pow(posprecedente-posseguidor, l_bigger));
			
			System.out.println("acelnueva1: "+acelnueva);
			
		}else{ if(distancia>distinfl){ //necesita aumentar la aceleracion
			
				acelnueva = (alpha_lower*Math.pow(velprecedentenueva, m_lower)*(velseguidor-velprecedente)) / (Math.pow(posprecedente-posseguidor, l_lower));
				System.out.println("acelnueva2: "+acelnueva);
				
			} else{ //al tener la distancia perfecta no necesita cambiar nada a = 0
				acelnueva = 0;
				System.out.println("acelnueva3: "+acelnueva);
				
			}
		}
		
		//controlamos que la aceleracion que obtenemos no sea mayor que la Amax del vehiculo
		if(acelnueva>actual.getVehiculo().getAmax()){
			acelnueva = actual.getVehiculo().getAmax();
			System.out.println("acelnuevaMAX: "+acelnueva);
			
		}
		
		//controlamos que la aceleracion que obtenemos no sea menor que la Amin del vehiculo
		if(acelnueva<actual.getVehiculo().getAmin()){
			acelnueva = actual.getVehiculo().getAmin();
			System.out.println("acelnuevaMIN: "+acelnueva);
			
		}
		
		velnueva = velseguidor + (acelnueva*incrementotiempo);		
		
		System.out.println("velnueva: "+velnueva);
		
		if (velnueva>=velocidaddeseada){
			acelnueva = 0; //para que mantenga la velocidad actual 
			velnueva = velocidaddeseada; // por si se pasa un poco de la velocidad deseada, asi conseguimos estabilidad
			System.out.println("velnuevaDESEADA: "+velnueva);
			
		}
		
		posnueva = posseguidor + (velnueva*incrementotiempo + ((acelnueva*incrementotiempo*incrementotiempo)/2));
		
		System.out.println("posnueva: "+posnueva);
		
		//si el coche esta al principio tiene que esperar en 0 a que el coche precedente se aleje un poco
		if(velnueva<0 && posnueva<0){
			velnueva = 0;
			posnueva = 0;
			acelnueva = 0;
		}
		
		System.out.println("FINAL: posnueva: "+posnueva+", acelnueva: "+acelnueva+" y velnueva: "+velnueva+"\n");
		
		actual.getEstadoNuevo().setAceleracion(acelnueva); 
		actual.getEstadoNuevo().setPosicion(posnueva); 
		actual.getEstadoNuevo().setVelocidad(velnueva); 
		
		return esc;
	}
}